#ifndef ARAL_INTERFACE_SOLVER_HPP
#define ARAL_INTERFACE_SOLVER_HPP

#include <string>
#include <vector>
#include <functional>
#include "aral/data_structure_definition.hpp"

namespace ARAL::interface {

/**
 * @class Solver
 * @brief 机器人模型的核心计算引擎，负责处理所有运动学和动力学求解。
 *
 * @details
 *  `Solver` 接口是ARAL算法库的“大脑”，它利用一个给定的 `Model` 实例来执行各种复杂的计算。
 *  与 `Model`（描述“机器人是什么”）和 `State`（描述“机器人现在在哪”）不同，`Solver` 专注于“计算”，
 *  它本身是无状态的，其所有计算都基于传入的参数（如关节角度、速度等）。
 *
 *  主要功能包括：
 *  - **运动学求解 (Kinematics)**:
 *    - **正解**: 从关节状态计算笛卡尔空间位姿、速度和加速度 (`kdCalForward...`)。
 *    - **逆解**: 从笛卡尔空间目标计算所需的关节状态 (`kdCalInverse...`)，支持解析解和迭代解，并提供构型选择。
 *    - **雅可比矩阵**: 计算机器人特定构型下的雅可比矩阵 (`kdCalJacobian`)。
 *
 *  - **动力学求解 (Dynamics)**:
 *    - **逆动力学**: 根据期望的运动计算所需的关节力矩 (`kdCalInverseDynamics`)。
 *    - **正动力学**: 根据施加的力矩计算产生的运动加速度 (`kdCalForwardDynamics`)。
 *    - **力矩分解**: 单独计算重力、科氏力/离心力等分量 (`kdCalGravityTorque`, `kdCalCoriolisTorque`)。
 *
 *  - **坐标变换 (Transformations)**:
 *    - 提供丰富的坐标系和姿态表示法（RPY, 四元数, 旋转矩阵）之间的转换工具 (`kdTransform...`, `kdChange...`)。
 *
 *  - **标定算法 (Calibration)**:
 *    - 封装了多种标定算法，如工具(TCP)标定、用户坐标系标定以及动力学参数辨识等 (`calib...`)。
 *
 *  每个 `Model` 实例在创建时都会附带一个专属的 `Solver` 实例，确保计算与模型参数严格对应。
 */
class Solver
{
public:
    virtual ~Solver() = default;

    //=============================================Kinematics and Dynamics Module================================================
    // 运动学接口, 有末端工具传参
    /**
     * @brief 正运动学, 根据当前关节角得到末端工具在基坐标系的位姿
     * @param q_in: 输入的关节角
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param real_fk: 基于理论 (false) / 实际 DH(true) 参数计算
     * @param T_b_e: 输出末端工具在基坐标系的位姿
     * @return: 返回值 < 0 表示计算失败，具体含义如下：
     * E_ROBOT_MODEL_CONSTRUCT: 机器人模型构建失败
     */
    virtual int kdCalForwardPosition(const RLJntArray& q_in, const RLPose& T_f_e, const bool& real_fk, RLPose& T_b_e) const = 0;

    /**
     * @brief 正运动学, 根据当前关节角得到各连杆坐标系的位姿
     * @param q_in: 输入的关节角
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param real_fk: 基于理论 (false) / 实际 DH(true) 参数计算
     * @param T_out: 输出各连杆坐标系在 基坐标系 下的　位姿, T01(link 1)-...->Ｔ06(flange)-->T07(end)
     * @param T_add: 输出各连杆坐标系在 上一个坐标系 下的 位姿，T01(link 1 in base)-...->Ｔ56(flange in link 5)-->T67(end in link 6)
     * @return: 返回值 < 0 表示计算失败，具体含义如下：
     * E_ROBOT_MODEL_CONSTRUCT: 机器人模型构建失败
     */
    virtual int kdCalForwardPositions(const interface::RLJntArray& q_in, const interface::RLPose& T_f_e, const bool& real_fk,
                                      std::vector<interface::RLPose>& T_out, std::vector<interface::RLPose>& T_add) const = 0;

    /**
     * @brief 正运动学, 根据关节位置和速度, 计算机械臂末端工具在基坐标系下的速度
     * @param q_in: 输入的关节角
     * @param dq_in: 输入的关节角速度
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param v_b_e: 输出末端工具在基坐标系的速度
     * @return: 返回值 < 0 表示计算失败，具体含义如下：
     * E_ROBOT_MODEL_CONSTRUCT: 机器人模型构建失败
     */
    virtual int kdCalForwardVelocity(const RLJntArray& q_in, const RLJntArray& dq_in, const RLPose& T_f_e, RLTwist& v_b_e) const = 0;

    /**
     * @brief 正运动学, 根据关节位置和速度, 计算机械臂各连杆坐标系在基坐标系下的速度
     * @param q_in: 输入的关节角
     * @param dq_in: 输入的关节角速度
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param v_out: 输出各连杆坐标系在 基坐标系 下的　速度, T01(link 1)-...->Ｔ06(flange)-->T07(end)
     * @return: 返回值 < 0 表示计算失败，具体含义如下：
     * E_ROBOT_MODEL_CONSTRUCT: 机器人模型构建失败
     */
    virtual int kdCalForwardVelocities(const interface::RLJntArray& q_in, const interface::RLJntArray& dq_in,
                                       const interface::RLPose& T_f_e, std::vector<interface::RLTwist>& v_out) const = 0;

    /**
     * @brief 由关节空间位置, 速度, 加速度得到机器人末端在笛卡尔空间的位姿, 速度, 加速度(在基坐标系下的描述)
     * @param q_in: 输入的关节角
     * @param dq_in: 输入的关节角速度
     * @param ddq_in: 输入的关节角加速度
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param T_b_e: 输出末端工具在基坐标系的位姿
     * @param v_b_e: 输出末端工具在基坐标系的速度
     * @param a_b_e: 输出末端工具在基坐标系的加速度
     * @return: 返回值 < 0 表示计算失败，具体含义如下：
     * E_ROBOT_MODEL_CONSTRUCT: 机器人模型构建失败
     */
    virtual int kdSpaceTransform(const interface::RLJntArray& q_in, const interface::RLJntArray& dq_in, const interface::RLJntArray& ddq_in,
                                 const interface::RLPose& T_f_e, interface::RLPose& T_b_e, interface::RLTwist& v_b_e, interface::RLTwist& a_b_e) const = 0;

    /**
     * @brief 逆运动学，根据机械臂末端位姿计算满足选解条件的关节角
     * @param config: 逆解输入信息, 参考 IKConfigInfo 结构体信息
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param real_ik: 基于理论 (false) / 实际 DH(true) 参数求解
     * @param q_out: 输出的关节角
     * @return 返回值 < 0, 表示计算失败, 具体定义如下:
     * 输入错误：
     * E_ROBOT_MODEL_CONSTRUCT：机器人模型构建失败
     * E_KD_JOINT_BOUND_UNREASONABLE：机械臂关节限制最小值大于最大值
     * E_IK_Q_REF_OUT_BOUND：参考关节角超过机械臂关节限制范围
     * 逆解错误：
     * E_IK_ANALYTICAL_SOLVER_FAILED：目标位姿不在工作空间内
     * E_IK_OUT_OF_SUBSPACE_CONFIG：不存在满足选解条件的逆解, 选解条件为: 同一子空间, 不能走笛卡尔空间轨迹到达目标位姿，可能原因为目标位姿接近奇异边界
     * E_IK_OUT_OF_NONE_CONFIG：不存在满足选解条件的逆解, 选解条件为: 距离参考关节角最近
     * E_IK_ITER_NOT_CONVERGENT：机械臂接近奇异
     * E_KD_SOL_OUT_JOINT_BOUND：解超过关节限制
     */
    virtual int kdCalInversePosition(const IKConfigInfo& config, const RLPose& T_f_e, const bool& real_ik, RLJntArray& q_out) const = 0;

    /**
     * @brief 逆运动学，根据机械臂末端位姿计算所有关节角（在机械臂的运动范围内, 由函数 mdlSetJointRange 设置）
     * @param config: 逆解输入信息, 参考 IKConfigInfo 结构体信息
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param real_ik: 基于理论 (false) / 实际 DH(true) 参数求解
     * @param q_out: 机械臂的逆解集合, 逆解的个数通过 q_out.size 得到
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalAllInversePositions(const IKConfigInfo& config, const RLPose& T_f_e, const bool& real_ik, std::vector<RLJntArray>& q_out)const = 0;

    /**
     * @brief 逆运动学，根据机械臂末端在 基坐标系 下的速度计算关节速度
     * @param q_in: 输入的关节角
     * @param v_in: 输入的末端工具的速度旋量, 在基坐标系下描述
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param dq_out: 输出的关节角速度
     * @return if < 0 则计算出错
     */
    virtual int kdCalInverseVelocity(const RLJntArray& q_in, const RLTwist& v_in, const RLPose& T_f_e, RLJntArray& dq_out)const = 0;

    /**
     * @brief 逆运动学,根据机械臂末端在 基坐标系 下的速度和加速度计算关节速度和加速度
     * @param q_in: 输入的关节角
     * @param v_in: 输入的末端工具的速度旋量, 在基坐标系下描述
     * @param a_in: 输入的末端工具的加速度旋量, 在基坐标系下描述
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param dq_out: 输出的关节角速度
     * @param ddq_out: 输出的关节角加速度
     * @return  if < 0 则计算出错
     */
    virtual int kdCalInverseAcceleration(const RLJntArray& q_in, const RLTwist& v_in, const RLTwist& a_in,
                                         const RLPose& T_f_e, RLJntArray& dq_out, RLJntArray& ddq_out)const = 0;

    /**
     * @brief 速度运动学，计算机械臂末端的雅克比矩阵在基坐标系的描述
     * @param q_in: 关节角位置
     * @param T_f_e: 输入的末端工具在法兰的位姿
     * @param jac: 该构型下对应的　末端工具　雅克比矩阵在　基坐标系　的描述, 维度: ROBOT_DOF*6 (矩阵横向索引)
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalJacobian(const RLJntArray& q_in, const RLPose& T_f_e, const bool& in_base_or_end, DoubleVec& jac)const = 0;

    /**
     * @brief 获得指定关节角对应的机械臂构型
     * @param q_in: 输入关节角
     * @return: 机械臂构型(详细见参考文档: http://git.aubo-robotics.cn:8001/aral_doc/doc_share/-/blob/master/机器人基础算法知识文档/doc/2.2.机械臂构型介绍.md)
     */
    virtual RobotConfiguration kdGetRobotConfiguration(const RLJntArray& q_in)const = 0;

    /**
     * @brief 计算在工作空间中的最大立方体
     * @param config: 选择的工作空间
     * @param T_f_e: 工具参数
     * @param centor: 立方体中心在基坐标系的描述
     * @param length: 立方体的边长
     * @return if < 0; something goes wrong
     */
    virtual int kdCalMaxCubicInWorkspace(const WorkspaceConfig& config, const RLPose& T_f_e, Array3d& centor, double& length) = 0;
    //==================================================动力学接口================================================

    /**
     * @brief 根据末端传感器数据计算得到作用在末端的　RLWrench (在 末端 坐标系下描述, 重力补偿工具)
     * @param ft_sensor: 传感器数值
     * @param joint_pos: 机械臂关节位置
     * @param joint_vel: 机械臂关节速度
     * @param joint_acc: 机械臂关节加速度
     *
     * @return: 作用在末端的 RLWrench (在 末端 坐标系下描述)
     */
    virtual int kdCalTCPWrenchFromEndSensor(const RLWrench& ft_sensor, const RLJntArray& joint_pos, const RLJntArray& joint_vel, const RLJntArray& joint_acc,
                                                 const RLPose& Tf_sensor,  const RLPose& Tf_tcp, const RLInertia& payload, RLWrench& ft_tcp)const = 0;

    /**
     * @brief 计算机械臂的逆动力学: Tau = M * qdd + C(q,qd) + G(q) + ft_ext
     * @param joint_pos: 关节位置
     * @param joint_vel: 关节速度
     * @param joint_acc: 关节加速度
     * @param ft_ext: 作用各个连杆坐标系的力和力矩,7 维 RLWrench, [link1 frame --> link6(flange) frame --> end frame]
     * @param joint_torque: 关节力矩
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalInverseDynamics(const RLJntArray& joint_pos, const RLJntArray& joint_vel, const RLJntArray& joint_acc, const RLPose& T_f_e,
                                     const std::vector<RLWrench>& ft_ext, const RLInertia& payload, RLJntArray& joint_torque)const = 0;

    /**
     * @brief 计算重力项在关节产生的力矩: G(q)
     * @param joint_pos: 关节位置
     * @param joint_torque: 关节力矩
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalGravityTorque(const RLJntArray& q, const RLInertia& payload, RLJntArray& robot_gravity, RLJntArray& payload_gravity) const = 0;

    /**
     * @brief 计算速度项在关节产生的力矩(包括科氏力和离心力): C(q,qd)
     * @param joint_pos: 关节位置
     * @param joint_vel: 关节速度
     * @param joint_torque: 关节力矩
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalCoriolisTorque(const RLJntArray& joint_pos, const RLJntArray& joint_vel, const RLInertia& payload, RLJntArray& joint_torque)const = 0;

    /**
     * @brief 计算机械臂在指定构型下的惯性矩阵: M
     * @param joint_pos: 关节角位置
     * @param M: 惯性矩阵
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalInertiaMatrix(const RLJntArray& joint_pos, const RLInertia& payload, DoubleVecVec& M)const = 0;

    /**
     * @brief 计算机械臂的正动力学
     * @param joint_pos: 关节位置
     * @param joint_vel: 关节速度
     * @param joint_torque: 关节力矩
     * @param ft_ext: 作用各个连杆坐标系的力和力矩, 7 维 RLWrench, [link1 frame --> link6(flange) frame --> end frame]
     * @param joint_acc: 关节加速度
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int kdCalForwardDynamics(const RLJntArray& joint_pos, const RLJntArray& joint_vel, const RLJntArray& joint_torque,  const RLPose& T_f_e,
                                     const std::vector<RLWrench>& ft_ext, const RLInertia& payload, RLJntArray& joint_acc)const = 0;

    /**
    * @brief 计算关节摩擦力, 通过接口 mdlSetJointFrictionPara 设置摩擦力模型后计算
    * @param joint_pos: 关节位置
    * @param joint_vel: 关节速度
    * @param joint_temperature: 关节温度
    * @param joint_torque: 关节力矩
    * @param friction: 计算的关节摩擦力，单位Nm
    * @return 返回值 < 0, 表示计算失败
    */
    virtual int kdCalFriction(const RLJntArray& joint_pos, const RLJntArray& joint_vel, const RLJntArray& joint_temperature, const RLJntArray& joint_torque, RLJntArray& friction) const = 0;

    //=================================================坐标系标定接口================================================

    /**
     * @brief 标定机器人末端特征坐标系的姿态在法兰坐标系的描述
     * @param method: 姿态标定方法, 具体含义见 CoordCalibMethod 结构体
     * @param F_b_f_calib_point: 法兰在基坐标系的位姿( = 3), 其顺序和标定类型的描述一致(保持法兰姿态不变)
     * 下面以标定类型为 O_X_XY 为例说明参数的含义:
     * 1) 运动末端特征坐标系原点到标定对齐尖点，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[0] 的输入;
     * 2) 保持末端特征坐标系姿态不动，沿末端特征坐标系X轴运动一定的距离，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[1] 的输入;
     * 3) 保持末端特征坐标系姿态不动，沿末端特征坐标系XY平面运动一定的距离，记录此时法兰在基坐标下的位姿，为 F_b_f_calib_point[2] 的输入;
     * @param R_f_end: 末端特征坐标系的姿态在法兰坐标系的描述
     * @return: 返回值 < 0, 表示标定失败
     */
    virtual int calibEndFrameOriInFlange(const CoordCalibMethod& method, const std::vector<RLPose>& F_b_f_calib_point, Array3d& R_f_end) = 0;

    /**
     * @brief 标定末端特征坐标系原点的位置在法兰坐标系的描述
     * @param F_b_f_calib_point: 法兰在基坐标下的位姿( >= 4), 保持末端特征坐标系原点不动，让机械臂运动到四个及以上的构型，记录此时法兰在基坐标下的位姿，即为此输入.
     * @param P_f_end: 末端特征坐标系的位置在法兰坐标系的描述
     * @param error: 标定后位置误差的平均值
     * @return: 返回值 < 0, 表示标定失败
     */
    virtual int calibEndFramePosInFlange(const std::vector<RLPose>& F_b_f_calib_point, Array3d& P_f_end, double& error) = 0;

    /**
     * @brief 标定远端特征坐标系的位置和姿态在基坐标系的描述
     * @param method: 姿态标定方法, 具体含义见 CoordCalibMethod 结构体
     * @param F_b_end_calib_point: 标定点 (末端坐标系) 在基坐标系下的位姿( = 3), 其顺序和标定类型的描述一致
     * 下面以标定类型为 O_X_XY 为例说明参数的含义:
     * 1) 运动到远端坐标系原点，记录此时末端坐标系在基坐标下的位姿，为 F_b_end_calib_point[0] 的输入;
     * 2) 沿远端特征坐标系X轴运动一定的距离，记录此时末端坐标系在基坐标下的位姿，为 F_b_end_calib_point[1] 的输入;
     * 3) 沿远端特征坐标系XY平面运动一定的距离，记录此时末端坐标系在基坐标下的位姿，为 F_b_end_calib_point[2] 的输入;
     * @param F_b_remote: 远端坐标系位姿在基坐标系的描述
     * @return: 返回值 < 0, 表示标定失败
     */
    virtual int calibRemoteFrameInBase(const CoordCalibMethod& method, const std::vector<RLPose>& F_b_end_calib_point, RLPose& F_b_remote) = 0;

    //===========================================运动学参数和动力学参数标定接口================================================

    /**
     * @brief 标定机器人运动学参数（dh参数）
     * @param nom_para: 运动学模型理论值
     * @param data_input: 输入的标定数据
     * @param est_para: 运动学模型估计值
     * @param res_error: 估计值误差, 分别为［标定数据位置误差，姿态误差，校验数据位置误差，姿态误差］
     * @return　返回值 < 0, 表示计算失败
     */
    virtual int calibRobotKinematicParameterBaseDual(const RobotKinCalibPara& nom_para, const std::vector<RobotKinCalibDualInput>& data_input,
                                                                  RobotKinCalibPara& est_para, std::vector<DataFeature>& res_error) = 0;

    /**
     * @brief 根据配置信息生成激励轨迹
     * @param config: 具体的配置信息, 根据实际辨识需求设置(参考 CalibConfig 结构体)
     * @param excitation_trajectory: 辨识的激励轨迹(需要运行该轨迹得到测量数据)
     * @param trajectory_para: 输出的激励轨迹的系数(在辨识动力学参数时算法需要输入该参数进行计算)
     * @return: 返回值 < 0, 表示计算失败(需要检查配置信息中的约束条件是否过于严格或者相互矛盾)
     */
     virtual int calibGenerateExcitationTrajectory(const CalibConfig& config, DoubleVecVec& excitation_trajectory, DoubleVec& trajectory_para) = 0;

    /**
     * @brief 根据末端 6 维力/力矩传感器的信息标定工具(在法兰坐标系的描述)和传感器的属性: 只能标定工具的质量和质心
     * @param T_b_f: 输入多种不同构型对应的法兰在基坐标系的描述, 这几种构型的姿态差异越大越好（个数应不小于 3）
     * @param wrenches: 在对应构型下测得的传感器的原始数据
     * @param result: 1. inertia: 工具在法兰坐标系下的惯性参数
     *                2. error: 辨识误差, 辨识参数映射到末端六维力 与 实际六维力的误差, 包括误差的average, max, rms值
     * @param offset: 辨识得到的传感器偏置
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int calibToolDynamicParameterWithFTSensor(const std::vector<RLPose>& T_b_f, const std::vector<RLWrench>& wrenches, ToolCalibResult& result, RLWrench& offset) = 0;

    /**
     * @brief 工具动力学参数辨识
     *        说明: 辨识步骤为先设置 CalibConfig 信息, 调用 calibGenerateExcitationTrajectory 得到激励轨迹及激励轨迹系数, 再运行激励轨迹得到测量数据,
     *             这样就可以得到该函数的输入 traj_feature 及 data_raw, 最后调用该函数进行参数辨识.
     * @param traj_feature: 轨迹特征信息, 具体包括如下
     *        traj_feature.config: 激励轨迹配置信息(辨识类型，初始关节角，运动轴以及采样频率具体参考 CalibConfig 结构体), 该信息需与 calibGenerateExcitationTrajectory 函数调用时的信息一致
     *                            可辨识类型type包括:
     *                            type = TOOL_JNT_CURRENT:          基于关节电流的工具动力学参数辨识
     *                            type = TOOL_END_SENSOR:           基于末端力传感器的工具动力学参数辨识
     *                            type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS:  基于关节电流的工具重力参数辨识(单向轨迹)
     *                            type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP:  基于关节电流的工具重力参数辨识(往返轨迹)
     *        traj_feature.coeff: 激励轨迹系数, 由 calibGenerateExcitationTrajectory 函数计算得来. 不采用激励轨迹的类型不用设置
     * @param data_raw: 关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base
     *             type = TOOL_JNT_CURRENT 时传入数据如下
     *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6不带载第一组数
     *                                                  .
     *                                                  .
     *                                                  .
     *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6不带载第n组数
     *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6带载第一组数
     *                                                  .
     *                                                  .
     *                                                  .
     *                                     q1 ... q6, qd1...qd6,...，F_base1 ... F_base6带载第n组数
     *             type = TOOL_GRAVITY_JNT_CURRENT_SINGLE_PASS 或
     *             type = TOOL_GRAVITY_JNT_CURRENT_RETURN_TRIP 时传入数据为
     *                                      q1 ... q6, qd1...qd6,...，F_base1 ... F_base6 带载数据，共n行
     * @param result:   1. inertia: 工具在法兰坐标系下的惯性参数(具体参考 RLInertia 结构体)
     *                  2. error: 辨识误差, 辨识参数映射到关节力矩/末端六维力 与 实际力矩/六维力的误差, 包括误差的average, max, rms值
     * @return 返回值 < 0, 表示计算失败
     */
    virtual int calibToolDynamicParameter(const CalibTrajectoryFeature& traj_feature, const DoubleVecVec& data_raw, ToolCalibResult& result) = 0;

    /**
     * @brief 机械臂本体动力学参数辨识
     *        说明: 辨识步骤为先设置 CalibConfig 信息, 调用 calibGenerateExcitationTrajectory 得到激励轨迹及激励轨迹系数, 再运行激励轨迹得到测量数据,
     *             这样就可以得到该函数的输入 traj_feature 及 data_raw, 最后调用该函数进行参数辨识.
     * @param traj_feature: 轨迹特征信息, 具体包括如下
     *        traj_feature.config: 激励轨迹配置信息(具体参考 CalibConfig 结构体), 该信息需与 calibGenerateExcitationTrajectory 函数调用时的信息一致
     *                            可辨识类型type包括:
     *                            type = BODY_JNT_CURRENT:         基于关节电流的本体动力学参数辨识(未实现)
     *                            type = BODY_BASE_SENSOR:         基于底座力传感器的本体动力学参数辨识(未实现)
     *                            type = BODY_INT_EXT:             基于底座力传感器和关节电流的本体动力学参数混合辨识(未实现)
     *                            type = BODY_GRAVITY_JNT_CURRENT: 基于关节电流的本体重力参数辨识(未实现)
     *                            type = BODY_GRAVITY_BASE_SENSOR: 基于底座力传感器的本体重力参数辨识
     *        traj_feature.coeff: 激励轨迹系数, 由 calibGenerateExcitationTrajectory 函数计算得来.
     * @param data_raw: 关节角q，速度qd，加速度qdd，关节电流current，末端力传感器F_end，底座力传感器F_base
     *               q1 ... q6, qd1...qd6,...，F_base1 ... F_base6 带载数据，共n行
     * @param res: 输出辨识的动力参数及误差.
     *        res.link_para: 连杆动力学参数，m1 mx1 my1 mz1 Jxx1 Jxy1 Jxz1 Jyy1 Jyz1 Jzz1 ... m6 ... Jzz6; 60个
     *        res.joint_rotor_inertia: 关节电机转子惯量; 重力辨识中无该参数
     *        res.joint_friction_para: 关节摩擦力参数; 重力辨识中无该参数
     *        res.error: 辨识误差, 辨识误差, 辨识参数映射到 关节力距/底座六维力 与 实际力距/六维力 的误差. 分为基于底座辨识的映射到底座,基于电流辨识的映射到关节.
     *                   包括误差的average, max, rms值
     * @return: 返回值 < 0 表示计算失败
     */
    virtual int calibBodyDynamicParameter(const CalibTrajectoryFeature& traj_feature, const DoubleVecVec& data_raw, BodyCalibResult& paraEst) = 0;

    /**
     * @brief 根据配置信息生成激励轨迹用于动力学参数辨识
     * @param config: 具体的配置信息(参考 CalibConfig 结构体)(需要传入给辨识接口函数)
     * @param excitation_trajectory: 辨识的激励轨迹(需要运行该轨迹得到测量数据)
     * @param trajpara: 激励轨迹的系数(需要传入给辨识接口函数)
     * @return: 返回值 < 0, 表示计算失败(需要检查配置信息中的约束条件是否过于严格或者相互矛盾)
     */
    ARAL_DEPRECATED("0.39") virtual int calibGenerateExcitationTrajectory(const CalibConfig& config, DoubleVecVec& excitation_trajectory) = 0;

    /**
     * @brief 根据关节信息（data_raw）标定工具的动力学参数在法兰坐标系的描述
     * @param type: 辨识的类型：TOOL_JNT_CURRENT    = 40,    //基于关节电流的工具动力学参数辨识
     *                        TOOL_GRAVITY_JNT_CURRENT    = 80,    //基于关节电流的工具重力参数辨识（惯性张量根据辨识的重力参数计算,惯性张量计算未实现）
     * @param data_raw: 关节角q，速度qd，加速度qdd，关节电流current，温度temp，末端力传感器F_end，底座力传感器F_base
     * @param res: 1. inertia: 工具在法兰坐标系下的惯性参数
     *             2. error: 标定误差: 基于末端力传感器辨识的映射到末端, 基于电流辨识的映射到关节. 较为直观的辨识精度指标为error.rms
     * @return
     */
    ARAL_DEPRECATED("0.39") virtual int calibToolDynamicParameterWithJointCurrent(const DynCalibType& type, const DoubleVecVec& data_raw, ToolCalibResult& res) = 0;

    /**
     * @brief 机械臂本体动力学参数辨识
     * @param type: 辨识的类型
     *        type = BODY_JNT_CURRENT:         基于关节电流的本体动力学参数辨识(未实现)
     *        type = BODY_BASE_SENSOR:         基于底座力传感器的本体动力学参数辨识(未实现)
     *        type = BODY_INT_EXT:             基于底座力传感器和关节电流的本体动力学参数混合辨识(未实现)
     *        type = BODY_GRAVITY_JNT_CURRENT: 基于关节电流的本体重力参数辨识(未实现)
     *        type = BODY_GRAVITY_BASE_SENSOR: 基于底座力传感器的本体重力参数辨识
     * @param data_raw: 关节角q，速度qd，加速度qdd，关节电流current，末端力传感器F_end，底座力传感器F_base,没有的参数置0
     * @param res: 输出辨识的动力参数及误差.
     *        res.link_para: 连杆动力学参数，m1 mx1 my1 mz1 Jxx1 Jxy1 Jxz1 Jyy1 Jyz1 Jzz1 ... m6 ... Jzz6; 60个
     *        res.joint_rotor_inertia: 关节电机转子惯量; 重力辨识中无该参数
     *        res.joint_friction_para: 关节摩擦力参数; 重力辨识中无该参数
     *        res.error: 辨识参数映射到关节/底座的误差.基于底座辨识的映射到底座,基于电流辨识的映射到关节. 较为直观的辨识精度指标为error.rms
     * @return: 返回值 < 0 表示计算失败
     */
    ARAL_DEPRECATED("0.39") virtual int calibBodyDynamicParameter(const DynCalibType& type, const DoubleVecVec& data_raw, BodyCalibResult& res) = 0;

    //===========================================几何体碰撞检测接口================================================
    /**
     * @brief 在给定关节构型下，检查机器人是否发生碰撞。
     * @param joint 要检查的机器人构型（关节角度）。
     * @param checkSelf
     *        - `true`: 只检查机器人自身的连杆之间的碰撞（自碰撞）。
     *        - `false`: 同时检查自碰撞和与环境中其他物体的碰撞。
     * @param result [out] 碰撞检测的结果。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int kdCheckRobotCollision(const RLJntArray& joint, const bool& checkSelf, GeometryCollisionResult& result)const = 0;

    /**
     * @brief 在给定关节构型下，计算机器人与环境或自身之间的最短距离。
     * @param joint 要计算的机器人构型（关节角度）。
     * @param threshold 距离计算阈值。只报告小于此阈值的距离，可用于优化性能。
     * @param checkSelf
     *        - `true`: 只计算机器人自身连杆之间的最短距离。
     *        - `false`: 同时计算自身连杆之间以及与环境物体之间的最短距离。
     * @param result [out] 最短距离的计算结果。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int kdCalMinimumDistance(const RLJntArray& joint, const double& threshold, const bool& checkSelf, GeometryDistanceResult& result)const = 0;

    /**
     * @brief [耗时接口] 计算机器人在标准负载和典型轨迹下的最大笛卡尔速度和加速度。
     * @details
     *   该函数用于评估机器人的性能。
     *   - **名义最大速度/加速度**: 在特定姿态下（二关节90°，其他0°）单关节（1轴）大幅度摆动时的性能。
     *   - **典型轨迹速度/加速度**: 在标定立方体对角线上走直线时的性能。
     * @param max_joint_vel 关节最大速度。
     * @param max_joint_acc 关节最大加速度。
     * @param max_joint_torue 关节最大力矩。
     * @param nominal_maximum_vel [out] 名义最大笛卡尔速度（线速度，角速度）。
     * @param nominal_maximum_acc [out] 名义最大笛卡尔加速度（线加速度，角加速度）。
     * @param typical_traj_vel [out] 典型轨迹最大笛卡尔速度。
     * @param typical_traj_acc [out] 典型轨迹最大笛卡尔加速度。
     * @return 成功返回0, 失败返回错误码。
     */
    virtual int kdCalMaximumCartesianVelocity(const RLJntArray& max_joint_vel, const RLJntArray& max_joint_acc, const RLJntArray& max_joint_torue, Array2d& nominal_maximum_vel, Array2d& nominal_maximum_acc, Array2d& typical_traj_vel, Array2d& typical_traj_acc)const = 0;

};//class Solver    
typedef std::shared_ptr<Solver> SolverPtr;
}   //namespace ARAL::interface

#endif // ARAL_INTERFACE_SOLVER_HPP